#include "valuepack.h"
#include "oslib.h"
#include "usart.h"
#include "Dog.h"
unsigned char bits[] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};

// const unsigned int VALUEPACK_INDEX_RANGE=VALUEPACK_BUFFER_SIZE<<3;
// const unsigned short  TXPACK_BYTE_SIZE = ((TX_BOOL_NUM+7)>>3)+TX_BYTE_NUM+(TX_SHORT_NUM<<1)+(TX_INT_NUM<<2)+(TX_FLOAT_NUM<<2);
// const unsigned short  RXPACK_BYTE_SIZE = ((RX_BOOL_NUM+7)>>3)+RX_BYTE_NUM+(RX_SHORT_NUM<<1)+(RX_INT_NUM<<2)+(RX_FLOAT_NUM<<2);
// unsigned short rx_pack_length = RXPACK_BYTE_SIZE+3;

long rxIndex = 0;
long rdIndex = 0;

// unsigned char *vp_rxbuff = (unsigned char*)malloc(sizeof(unsigned char)*VALUEPACK_BUFFER_SIZE);

// unsigned char *vp_txbuff = (unsigned char*)malloc(sizeof(unsigned char)*(TXPACK_BYTE_SIZE+3));

unsigned char vp_rxbuff[VALUEPACK_BUFFER_SIZE];
unsigned char vp_txbuff[TXPACK_BYTE_SIZE + 3];

uint8_t usart3_rx_flag = 0;
uint8_t usart3_rx_buffer[128];
uint8_t usart3_tx_buffer[128];
uint16_t usart3_tx_len = 128;

DMA_InitTypeDef dma;

struct TxPack txpack;
struct RxPack rxpack;

void initValuePack()
{
	/* USART3接收 */
	if (HAL_UART_Receive_DMA(&huart3, (uint8_t *)&vp_rxbuff, VALUEPACK_BUFFER_SIZE) != HAL_OK)
	{
		Error_Handler();
	}
	/* 开启空闲接收中断 */
	__HAL_UART_ENABLE_IT(&huart3, UART_IT_IDLE);
}

unsigned short this_index = 0;
unsigned short last_index = 0;
unsigned short rdi, rdii, idl, idi;
uint32_t idc;
unsigned int err = 0;
unsigned char sum = 0;
unsigned char isok;

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// procValuePack 用来处理手机传来的数据
//

unsigned char readValuePack(struct RxPack *rx_pack_ptr)
{
	isok = 0;
	this_index = VALUEPACK_BUFFER_SIZE - hdma_usart3_rx.Instance->NDTR;
	if (this_index < last_index)
		rxIndex += VALUEPACK_BUFFER_SIZE + this_index - last_index;
	else
		rxIndex += this_index - last_index;
	while (rdIndex < (rxIndex - ((rx_pack_length))))
		rdIndex += rx_pack_length;
	while (rdIndex <= (rxIndex - rx_pack_length))
	{

		rdi = rdIndex % VALUEPACK_BUFFER_SIZE;
		rdii = rdi + 1;
		if (vp_rxbuff[rdi] == PACK_HEAD)
		{
			if (vp_rxbuff[(rdi + RXPACK_BYTE_SIZE + 2) % VALUEPACK_BUFFER_SIZE] == PACK_TAIL)
			{
				//  计算校验和
				sum = 0;
				for (short s = 0; s < RXPACK_BYTE_SIZE; s++)
				{
					rdi++;
					if (rdi >= VALUEPACK_BUFFER_SIZE)
						rdi -= VALUEPACK_BUFFER_SIZE;
					sum += vp_rxbuff[rdi];
				}
				rdi++;
				if (rdi >= VALUEPACK_BUFFER_SIZE)
					rdi -= VALUEPACK_BUFFER_SIZE;

				if (sum == vp_rxbuff[rdi])
				{
//  提取数据包数据 一共有五步， bool byte short int float

// 1. bool
#if RX_BOOL_NUM > 0

					idc = (uint32_t)rx_pack_ptr->bools;
					idl = (RX_BOOL_NUM + 7) >> 3;
					for (idi = 0; idi < idl; idi++)
					{
						if (rdii >= VALUEPACK_BUFFER_SIZE)
							rdii -= VALUEPACK_BUFFER_SIZE;
						(*((unsigned char *)idc)) = vp_rxbuff[rdii];
						rdii++;
						idc++;
					}
#endif
// 2.byte
#if RX_BYTE_NUM > 0
					idc = (uint32_t)(rx_pack_ptr->bytes);
					idl = RX_BYTE_NUM;
					for (idi = 0; idi < idl; idi++)
					{
						if (rdii >= VALUEPACK_BUFFER_SIZE)
							rdii -= VALUEPACK_BUFFER_SIZE;
						(*((unsigned char *)idc)) = vp_rxbuff[rdii];
						rdii++;
						idc++;
					}
#endif
// 3.short
#if RX_SHORT_NUM > 0
					idc = (uint32_t)(rx_pack_ptr->shorts);
					idl = RX_SHORT_NUM << 1;
					for (idi = 0; idi < idl; idi++)
					{
						if (rdii >= VALUEPACK_BUFFER_SIZE)
							rdii -= VALUEPACK_BUFFER_SIZE;
						(*((unsigned char *)idc)) = vp_rxbuff[rdii];
						rdii++;
						idc++;
					}
#endif
// 4.int
#if RX_INT_NUM > 0
					idc = (uint32_t)(&(rx_pack_ptr->integers[0]));
					idl = RX_INT_NUM << 2;
					for (idi = 0; idi < idl; idi++)
					{
						if (rdii >= VALUEPACK_BUFFER_SIZE)
							rdii -= VALUEPACK_BUFFER_SIZE;
						(*((unsigned char *)idc)) = vp_rxbuff[rdii];
						rdii++;
						idc++;
					}
#endif
// 5.float
#if RX_FLOAT_NUM > 0
					idc = (uint32_t)(&(rx_pack_ptr->floats[0]));
					idl = RX_FLOAT_NUM << 2;
					for (idi = 0; idi < idl; idi++)
					{
						if (rdii >= VALUEPACK_BUFFER_SIZE)
							rdii -= VALUEPACK_BUFFER_SIZE;
						(*((unsigned char *)idc)) = vp_rxbuff[rdii];
						rdii++;
						idc++;
					}
#endif
					err = rdii;
					rdIndex += rx_pack_length;
					isok = 1;
				}
				else
				{
					rdIndex++;
					err++;
				}
			}
			else
			{
				rdIndex++;
				err++;
			}
		}
		else
		{
			rdIndex++;
			err++;
		}
	}
	last_index = this_index;
	return isok;
}

void sendBuffer(unsigned char *p, unsigned short length)
{
	dma_send(p, (unsigned int)length);
}

unsigned short loop;
unsigned char valuepack_tx_bit_index;
unsigned char valuepack_tx_index;

void sendValuePack(struct TxPack *tx_pack_ptr)
{
	int i;
	vp_txbuff[0] = 0xa5;
	sum = 0;
	//  由于结构体中不同类型的变量在内存空间的排布不是严格对齐的，中间嵌有无效字节，因此需要特殊处理

	valuepack_tx_bit_index = 0;
	valuepack_tx_index = 1;

#if TX_BOOL_NUM > 0

	for (loop = 0; loop < TX_BOOL_NUM; loop++)
	{
		if (tx_pack_ptr->bools[loop])
			vp_txbuff[valuepack_tx_index] |= 0x01 << valuepack_tx_bit_index;
		else
			vp_txbuff[valuepack_tx_index] &= ~(0x01 << valuepack_tx_bit_index);

		valuepack_tx_bit_index++;
		if (valuepack_tx_bit_index >= 8)
		{
			valuepack_tx_bit_index = 0;
			valuepack_tx_index++;
		}
	}
	if (valuepack_tx_bit_index != 0)
		valuepack_tx_index++;
#endif

#if TX_BYTE_NUM > 0

	for (loop = 0; loop < TX_BYTE_NUM; loop++)
	{
		vp_txbuff[valuepack_tx_index] = tx_pack_ptr->bytes[loop];
		valuepack_tx_index++;
	}

#endif

#if TX_SHORT_NUM > 0
	for (loop = 0; loop < TX_SHORT_NUM; loop++)
	{
		vp_txbuff[valuepack_tx_index] = tx_pack_ptr->shorts[loop] & 0xff;
		vp_txbuff[valuepack_tx_index + 1] = tx_pack_ptr->shorts[loop] >> 8;
		valuepack_tx_index += 2;
	}
#endif

#if TX_INT_NUM > 0
	for (loop = 0; loop < TX_INT_NUM; loop++)
	{
		i = tx_pack_ptr->integers[loop];

		vp_txbuff[valuepack_tx_index] = i & 0xff;
		vp_txbuff[valuepack_tx_index + 1] = (i >> 8) & 0xff;
		vp_txbuff[valuepack_tx_index + 2] = (i >> 16) & 0xff;
		vp_txbuff[valuepack_tx_index + 3] = (i >> 24) & 0xff;

		valuepack_tx_index += 4;
	}
#endif

#if TX_FLOAT_NUM > 0
	for (loop = 0; loop < TX_FLOAT_NUM; loop++)
	{
		i = *(int *)(&(tx_pack_ptr->floats[loop]));

		vp_txbuff[valuepack_tx_index] = i & 0xff;
		vp_txbuff[valuepack_tx_index + 1] = (i >> 8) & 0xff;
		vp_txbuff[valuepack_tx_index + 2] = (i >> 16) & 0xff;
		vp_txbuff[valuepack_tx_index + 3] = (i >> 24) & 0xff;

		valuepack_tx_index += 4;
	}
#endif

	for (unsigned short d = 1; d <= TXPACK_BYTE_SIZE; d++)
		sum += vp_txbuff[d];
	vp_txbuff[TXPACK_BYTE_SIZE + 1] = sum;
	vp_txbuff[TXPACK_BYTE_SIZE + 2] = 0x5a;
	sendBuffer(vp_txbuff, TXPACK_BYTE_SIZE + 3);
}

void UsartReceive_IDLE(UART_HandleTypeDef *huart)
{

	if ((__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE) != RESET))
	{
	}
}
void dma_send(unsigned char *buffer, unsigned int length)
{

	while (HAL_DMA_GetState(&hdma_usart3_tx) == HAL_DMA_STATE_BUSY)
	{
	}

	// if (huart3.gState != HAL_UART_STATE_READY)
	// 	HAL_UART_AbortTransmit(&huart3);

	// __HAL_DMA_DISABLE(&hdma_usart3_tx);

	HAL_UART_Transmit_DMA(&huart3, buffer, length);
	huart3.gState = HAL_UART_STATE_READY;
}

void bluetooth_handle_exe()
{
	if (readValuePack(&rxpack))
	{
		// uprintf("	readValuePack time %.1f\n\r", osKernelGetTickCount() / 1000.0);

		static int last_mode = 0;
		speed_mode = rxpack.bools[0];

		Dog.v_z = rxpack.floats[0];
		Dog.v_x = rxpack.floats[1];
		delta_z = rxpack.floats[2];
		delta_x = rxpack.floats[3];
		Dog.speed = sqrt(pow(Dog.v_x, 2) + pow(Dog.v_z, 2));
		if (speed_mode == 0)
		{
			GAIT_T = rxpack.floats[5];
			GAIT_S = Dog.speed * GAIT_T;
		}
		else
		{
			GAIT_S = rxpack.floats[4];
			GAIT_T = GAIT_S / Dog.speed;
		}

		// GAIT_S = rxpack.floats[4];
		// GAIT_T = rxpack.floats[5];
		Dog.target_yaw = rxpack.floats[6];
		Dog.target_pitch = rxpack.floats[7];
		Dog.target_roll = rxpack.floats[8];
		// uprintf("Dog.v_z %.4f Dog.v_x %.4f delta_x %.3f delta_z %.3f\n\r", Dog.v_z, Dog.v_x, delta_x, delta_z);
		turn_angle = rxpack.floats[9];
		if (turn_angle > 0)
		{
			delta_dh[1] = turn_angle;
			delta_dh[2] = turn_angle;
			delta_dh[0] = 0;
			delta_dh[3] = 0;
		}
		else if (turn_angle < 0)
		{
			delta_dh[1] = 0;
			delta_dh[2] = 0;
			delta_dh[0] = -turn_angle;
			delta_dh[3] = -turn_angle;
		}

		Dog_Gait_Turn();
		if (last_mode != rxpack.shorts[0])
		{
			Dog.ctrl_mode = rxpack.shorts[0];
			last_mode = rxpack.shorts[0];
		}

		// txpack.bools[0] = rxpack.bools[0];
		// txpack.bytes[0] = rxpack.bytes[0];
		// txpack.shorts[0] = rxpack.shorts[0];
		// txpack.integers[0] = rxpack.integers[0];
		// txpack.floats[0] = rxpack.floats[0];
		// txpack.floats[0] = Dog.now_yaw;
		// txpack.floats[1] = Dog.now_pitch;
		// txpack.floats[2] = Dog.now_roll;

		// uprintf("remote YPR: %.4f %.4f %.4f\n\r", Dog.target_yaw, Dog.target_pitch, Dog.target_roll);
		// uprintf("switch dog mode to %d", Dog.ctrl_mode);
	}

	txpack.bytes[0] = 1;
	txpack.floats[0] = GAIT_T;
	txpack.floats[1] = GAIT_S;
	sendValuePack(&txpack);
}
